/*
 * Compensation.h
 *
 *  Created on: Oct 4, 2024
 *      Author: Grasping
 */

#ifndef MY_INC_PHY_PHY_COMPENSATION_H_
#define MY_INC_PHY_PHY_COMPENSATION_H_

#include <stdint.h>
#include <stdio.h>

float               Compensate(uint16_t raw_data, float slope, float intercept);
static inline float Compensate_Inline(uint16_t raw_data, float slope, float intercept)
{
    return ((float)slope * raw_data) + intercept;
}
#define COMPENSATE(raw_data, slope, intercept) ((slope) * (float)(raw_data) + (intercept))
/**
 * @brief 可变步长控制器的结构体
 */
typedef enum
{
    VARIABLE_STEP_STATE_CALE = 0,     // 计算步长
    VARIABLE_STEP_STATE_RUNNING = 1,  // 运行中
    VARIABLE_STEP_STATE_COMPLETE = 2, // 完成
} VariableStep_state;

typedef struct
{
    float    previousReference; // 上一个参考值
    uint16_t currentStep;       // 当前步长
    uint16_t totalSteps;        // 总步长
    float    incrementStep;     // 每一步的增量
    float    currentTemp;       // 当前参考值
    float    currentRef;        // 当前的参考值

    VariableStep_state state; // 0: 运行中 1: 停止

} VariableStep_Typedef;

void VariableStep_Init(VariableStep_Typedef *VariableStep, float frequency, float duration, float start_num);
void VariableStep_Reset(VariableStep_Typedef *VariableStep, float start_num);
static inline float VariableStep_Update_Inline(VariableStep_Typedef *VariableStep, float currentReference)
{
    switch (VariableStep->state)
    {
    case VARIABLE_STEP_STATE_CALE:
        VariableStep->incrementStep = (VariableStep->currentRef - VariableStep->currentTemp) /
                                      (float)VariableStep->totalSteps; // 重新计算每一步的增量
        VariableStep->previousReference = VariableStep->currentRef;    // 更新前一时刻的参考值为当前期望值
        VariableStep->currentStep = 0;                                 // 重置当前步数和完成标志
        VariableStep->state = VARIABLE_STEP_STATE_RUNNING;             // 设置状态为运行
        break;
    case VARIABLE_STEP_STATE_RUNNING:
        VariableStep->currentTemp += VariableStep->incrementStep; // 逐步增加当前参考值
        if (VariableStep->currentStep++ > VariableStep->totalSteps)
        {
            VariableStep->currentTemp = VariableStep->previousReference; // 确保最终参考值的准确性
            VariableStep->state = VARIABLE_STEP_STATE_COMPLETE;          // 设置状态为完成
        }
        break;
    case VARIABLE_STEP_STATE_COMPLETE:
        VariableStep->state = VARIABLE_STEP_STATE_COMPLETE; // 设置状态为计算步长
        VariableStep->currentRef = currentReference;
        if (VariableStep->currentRef != VariableStep->previousReference)
            VariableStep->state = VARIABLE_STEP_STATE_CALE;
        break;
    default:
        VariableStep->state = VARIABLE_STEP_STATE_COMPLETE; // 设置状态为计算步长
        break;
    }
    return VariableStep->currentTemp;
}
#endif /* MY_INC_PHY_PHY_COMPENSATION_H_ */
